dynamics_controls Module

Uses

- diffeq
- nonlin_polynomials
- lapack
- ieee_arithmetic
- dynamics_error_handling
- iso_fortran_env
- linalg

Contents

Interfaces

operator(*) ss_excitation state_space transfer_function

Derived Types

state_space transfer_function

Functions

lti_solve

Interfaces

public interface operator(*)

private pure function tf_tf_mult(x, y) result(rst)

Multiplies two transfer functions.

Arguments

Type	Intent	Optional	Attributes		Name
class(transfer_function),	intent(in)			::	x	The left-hand-side argument.
class(transfer_function),	intent(in)			::	y	The right-hand-side argument.

Return Value type(transfer_function)

The resulting transfer function.

private pure function poly_tf_mult(x, y) result(rst)

Multiplies a polynomial and a transfer function to result in a new transfer function.

Arguments

Type	Intent	Optional	Attributes		Name
class(polynomial),	intent(in)			::	x	The left-hand-side argument.
class(transfer_function),	intent(in)			::	y	The right-hand-side argument.

Return Value type(transfer_function)

The resulting transfer function.

private pure function tf_poly_mult(x, y) result(rst)

Multiplies a transfer function and a polynomial to result in a new transfer function.

Arguments

Type	Intent	Optional	Attributes		Name
class(transfer_function),	intent(in)			::	x	The left-hand-side argument.
class(polynomial),	intent(in)			::	y	The right-hand-side argument.

Return Value type(transfer_function)

The resulting transfer function.

private pure function tf_scalar_mult(x, y) result(rst)

Multiplies a transfer function by a scalar value.

Arguments

Type	Intent	Optional	Attributes		Name
class(transfer_function),	intent(in)			::	x	The left-hand-side argument.
real(kind=real64),	intent(in)			::	y	The right-hand-side argument.

Return Value type(transfer_function)

The resulting transfer function.

private pure function scalar_tf_mult(x, y) result(rst)

Multiplies a transfer function by a scalar value.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	x	The left-hand-side argument.
class(transfer_function),	intent(in)			::	y	The right-hand-side argument.

Return Value type(transfer_function)

The resulting transfer function.

interface

public subroutine ss_excitation(t, u, args)

A routine for computing the excitation vector for a state-space model.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	t	The time value at which to compute the excitation.
real(kind=real64),	intent(out),		dimension(:)	::	u	The excitation vector.
class(*),	intent(inout),	optional		::	args	An optional argument used to pass objects in and out of the routine.

public interface state_space

private pure function state_space_init(m, b, k, n_out) result(rst)

Initializes the state space model. For the second-order mechanical system $M\ddot{q}+B\dot{q}+Kq=u,$ the state $x=[q^T\ \dot{q}^T]^T$ satisfies $\dot{x}=Ax+Bu,\qquad y=Cx+Du.$

The output matrix $C$ is initialized to one, and the feedthrough matrix $D$ is initialized to zero.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in),		dimension(:,:)	::	m	The N-by-N mass matrix.
real(kind=real64),	intent(in),		dimension(size(m, 1), size(m, 2))	::	b	The N-by-N damping matrix.
real(kind=real64),	intent(in),		dimension(size(m, 1), size(m, 2))	::	k	The N-by-N stiffness matrix.
integer(kind=int32),	intent(in),	optional		::	n_out	The number of outputs. The default is 1.

Return Value type(state_space)

The [[state_space]] model.

private pure function state_space_init_scalar(m, b, k) result(rst)

Initializes the state space model. The scalar realization corresponds to $m\ddot{q}+b\dot{q}+kq=u,\qquad A=\begin{bmatrix}0&1\\-k/m&-b/m\end{bmatrix},\quad B=\begin{bmatrix}0\\1/m\end{bmatrix}.$

The output matrix $C$ is initialized to one, and the feedthrough matrix $D$ is initialized to zero.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	m	The mass.
real(kind=real64),	intent(in)			::	b	The damping.
real(kind=real64),	intent(in)			::	k	The stiffness.

Return Value type(state_space)

The [[state_space]] model.

private pure function state_space_init_matrices(a, b, c, d) result(rst)

Initializes the state space model. The stored realization uses the continuous-time equations $\dot{x}=Ax+Bu,\qquad y=Cx+Du.$

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in),		dimension(:,:)	::	a	The N-by-N dynamics matrix.
real(kind=real64),	intent(in),		dimension(:,:)	::	b	The N-by-M input matrix.
real(kind=real64),	intent(in),		dimension(:,:)	::	c	The P-by-N output matrix.
real(kind=real64),	intent(in),		dimension(:,:)	::	d	The P-by-M feedthrough matrix.

Return Value type(state_space)

The resulting [[state_space]] object.

private pure function state_space_init_pid(kp, ki, kd, tau, a, b, c, d) result(rst)

Initializes a state-space model that employs a closed-loop PID controller.

The PID model is augmented into the plant model as follows.

$e = r - y$ $\dot{x_1} = e$ $\dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau}$ $u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - x_{3} \right)$ $\alpha = K_{p} + \frac{K_{d}}{\tau}$ $\beta = \frac{1}{1 + \alpha D}$ $x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} \right]$ $\dot{x} = A_{cl} x + B_{cl} r$ $y = C_{cl} x + D_{cl} r$

Where the augmented matrices are as follows.

$A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & -\frac{\beta K_{i}}{\tau} D & -\frac{1}{\tau} + \frac{\beta K_{d}}{\tau^{2}} D \end{matrix} \right]$ $B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) \end{matrix} \right]$ $C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right]$ $D_{cl} = \alpha \beta D$

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	kp	The proportional gain term.
real(kind=real64),	intent(in)			::	ki	The integral gain term.
real(kind=real64),	intent(in)			::	kd	The derivative gain term.
real(kind=real64),	intent(in)			::	tau	The time constant of the first order derivative filter $K_{d} \frac{s}{\tau s + 1}$ .
real(kind=real64),	intent(in),		dimension(:,:)	::	a	The N-by-N dynamics matrix for the plant.
real(kind=real64),	intent(in),		dimension(size(a, 1), 1)	::	b	The N-by-1 input matrix for the plant.
real(kind=real64),	intent(in),		dimension(1, size(a, 1))	::	c	The 1-by-N output matrix for the plant.
real(kind=real64),	intent(in),		dimension(1, 1)	::	d	The 1-by-1 feedthrough matrix for the plant.

Return Value type(state_space)

The resulting [[state_space]] object.

private pure function state_space_init_pid_plant(kp, ki, kd, tau, plant) result(rst)

Initializes a state-space model that employs a closed-loop PID controller.

The PID model is augmented into the plant model as follows.

$e = r - y$ $\dot{x_1} = e$ $\dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau}$ $u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - x_{3} \right)$ $\alpha = K_{p} + \frac{K_{d}}{\tau}$ $\beta = \frac{1}{1 + \alpha D}$ $x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} \right]$ $\dot{x} = A_{cl} x + B_{cl} r$ $y = C_{cl} x + D_{cl} r$

Where the augmented matrices are as follows.

$A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & -\frac{\beta K_{i}}{\tau} D & \frac{1}{\tau} \left( 1 + \frac{\beta K_{d}}{\tau^{2}} D \right) \end{matrix} \right]$ $B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) \end{matrix} \right]$ $C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right]$ $D_{cl} = \alpha \beta D$

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in)			::	kp	The proportional gain term.
real(kind=real64),	intent(in)			::	ki	The integral gain term.
real(kind=real64),	intent(in)			::	kd	The derivative gain term.
real(kind=real64),	intent(in)			::	tau	The time constant of the first order derivative filter $K_{d} \frac{s}{\tau s + 1}$ .
class(state_space),	intent(in)			::	plant	The plant model.

Return Value type(state_space)

The resulting [[state_space]] object.

public interface transfer_function

private pure function init_tf_array(y, x) result(rst)

Initializes a new transfer function.

Arguments

Type	Intent	Optional	Attributes		Name
real(kind=real64),	intent(in),		dimension(:)	::	y	The numerator polynomial $Y(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ . The polynomial coefficients are stored in acending order such that $y_1 + y_2 s + y_3 s^2 ...$ .
real(kind=real64),	intent(in),		dimension(:)	::	x	The denominator polynomial $X(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ . The polynomial coefficients are stored in acending order such that $x_1 + x_2 s + x_3 s^2 ...$ .

Return Value type(transfer_function)

The resulting [[transfer_function]].

private pure function init_tf_poly(y, x) result(rst)

Initializes a new transfer function.

Arguments

Type	Intent	Optional	Attributes		Name
class(polynomial),	intent(in)			::	y	The numerator polynomial $Y(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ .
class(polynomial),	intent(in)			::	x	The denominator polynomial $X(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ .

Return Value type(transfer_function)

The resulting [[transfer_function]].

Derived Types

type, public :: state_space

Defines a state-space representation of a dynamic system. This implementation takes the form:

Components

Type	Visibility	Attributes		Name		Initial
real(kind=real64),	public,	allocatable, dimension(:,:)	::	A			The N-by-N dynamics matrix, where N is the number of state variables.
real(kind=real64),	public,	allocatable, dimension(:,:)	::	B			The N-by-M input matrix, where M is the number of inputs.
real(kind=real64),	public,	allocatable, dimension(:,:)	::	C			The P-by-N output matrix, where P is the number of outputs.
real(kind=real64),	public,	allocatable, dimension(:,:)	::	D			The P-by-M feedthrough matrix.

Constructor

private pure function state_space_init (m, b, k, n_out)	Initializes the state space model. For the second-order mechanical system $M\ddot{q}+B\dot{q}+Kq=u,$ the state $x=[q^T\ \dot{q}^T]^T$ satisfies $\dot{x}=Ax+Bu,\qquad y=Cx+Du.$ The output matrix $C$ is initialized to one, and the feedthrough matrix $D$ is initialized to zero.
private pure function state_space_init_scalar (m, b, k)	Initializes the state space model. The scalar realization corresponds to $m\ddot{q}+b\dot{q}+kq=u,\qquad A=\begin{bmatrix}0&1\\-k/m&-b/m\end{bmatrix},\quad B=\begin{bmatrix}0\\1/m\end{bmatrix}.$ The output matrix $C$ is initialized to one, and the feedthrough matrix $D$ is initialized to zero.
private pure function state_space_init_matrices (a, b, c, d)	Initializes the state space model. The stored realization uses the continuous-time equations $\dot{x}=Ax+Bu,\qquad y=Cx+Du.$
private pure function state_space_init_pid (kp, ki, kd, tau, a, b, c, d)	Initializes a state-space model that employs a closed-loop PID controller. The PID model is augmented into the plant model as follows. $e = r - y$ $\dot{x_1} = e$ $\dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau}$ $u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - x_{3} \right)$ $\alpha = K_{p} + \frac{K_{d}}{\tau}$ $\beta = \frac{1}{1 + \alpha D}$ $x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} \right]$ $\dot{x} = A_{cl} x + B_{cl} r$ $y = C_{cl} x + D_{cl} r$ Where the augmented matrices are as follows. $A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & -\frac{\beta K_{i}}{\tau} D & -\frac{1}{\tau} + \frac{\beta K_{d}}{\tau^{2}} D \end{matrix} \right]$ $B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) \end{matrix} \right]$ $C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right]$ $D_{cl} = \alpha \beta D$
private pure function state_space_init_pid_plant (kp, ki, kd, tau, plant)	Initializes a state-space model that employs a closed-loop PID controller. The PID model is augmented into the plant model as follows. $e = r - y$ $\dot{x_1} = e$ $\dot{x_3} = -\frac{x_3}{\tau} + \frac{e}{\tau}$ $u = K_{i} x_{i} + K_{p} e + \frac{K_{d}}{\tau} \left(e - x_{3} \right)$ $\alpha = K_{p} + \frac{K_{d}}{\tau}$ $\beta = \frac{1}{1 + \alpha D}$ $x = \left[ \begin{matrix} x_{p} \\ x_{1} \\ x_{3} \end{matrix} \right]$ $\dot{x} = A_{cl} x + B_{cl} r$ $y = C_{cl} x + D_{cl} r$ Where the augmented matrices are as follows. $A_{cl} = \left[ \begin{matrix} A - \alpha \beta B C & \beta K_{i} B & -\frac{\beta K_{d}}{\tau} B \\ -C + \alpha \beta D C & -\beta K_{i} D & \frac{\beta K_{d}}{\tau} D \\ \frac{1}{\tau}\left(-C + \alpha \beta D C \right) & -\frac{\beta K_{i}}{\tau} D & \frac{1}{\tau} \left( 1 + \frac{\beta K_{d}}{\tau^{2}} D \right) \end{matrix} \right]$ $B_{cl} = \left[ \begin{matrix} \alpha \beta B \\ 1 - \alpha \beta D \\ \frac{1}{\tau} \left( 1 - \alpha \beta D \right) \end{matrix} \right]$ $C_{cl} = \left[ \begin{matrix} C - \alpha \beta D C & \beta K_{i} D & -\frac{\beta K_{d}}{\tau} D \end{matrix} \right]$ $D_{cl} = \alpha \beta D$

Type-Bound Procedures

procedure , public :: evaluate_derivatives => ss_eval_deriv Function
procedure , public :: evaluate_output => ss_eval_output Function
procedure , public :: poles => ss_poles Function
generic, public :: transfer_function => ss_transfer_fcn, ss_transfer_fcn_omega, ss_transfer_fcn_array, ss_transfer_fcn_omega_array
procedure , public :: zeros => ss_zeros Function

type, public :: transfer_function

Defines a transfer function for a continuous system of the form $H(s) = \frac{Y(s)}{X(s)}$ .

Components

Type	Visibility	Attributes		Name		Initial
type(polynomial),	public		::	X			The denominator polynomial $X(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ . The polynomial coefficients are stored in acending order such that $x_1 + x_2 s + x_3 s^2 ...$ .
type(polynomial),	public		::	Y			The numerator polynomial $Y(s)$ in $H(s) = \frac{Y(s)}{X(s)}$ . The polynomial coefficients are stored in acending order such that $y_1 + y_2 s + y_3 s^2 ...$ .

Constructor

private pure function init_tf_array (y, x)	Initializes a new transfer function.
private pure function init_tf_poly (y, x)	Initializes a new transfer function.

Type-Bound Procedures

generic, public :: evaluate => tf_eval_omega, tf_eval_s
procedure , public :: poles => tf_poles Function
procedure , public :: to_ccf_state_space => tf_to_ccf_statespace Function
procedure , public :: to_ocf_state_space => tf_to_ocf_statespace Function
procedure , public :: zeros => tf_zeros Function

Functions

public function lti_solve(mdl, u, t, ic, solver, args) result(rst)

Solves the LTI system given by the specified state space model.

Arguments

Type	Intent	Optional	Attributes		Name
class(state_space),	intent(in),		target	::	mdl	The state_space model to solve.
procedure(ss_excitation),	intent(in),		pointer	::	u	The routine used to compute the excitation vector.
real(kind=real64),	intent(in),		dimension(:)	::	t	The time points at which to compute the solution. The array must have at least 2 values; however, more may be specified. If only 2 values are specified, the integrator will compute the solution at those points, but it will also return any intermediate integration steps that may be required. However, if more than 2 points are given, the integrator will return the solution values only at the specified time points.
real(kind=real64),	intent(in),		dimension(:)	::	ic	The initial condition vector. This array must be the same size as the number of state variables.
class(ode_integrator),	intent(in),	optional,	target	::	solver	The ODE solver to utilize. If not specified, the default solver is a 4th/5th order Runge-Kutta integrator.
class(*),	intent(inout),	optional		::	args	An optional container for arguments to pass to the excitation routine.

Return Value real(kind=real64), allocatable, dimension(:,:)

The solution. The time points at which the solution was evaluated are stored in the first column and the output(s) are stored in the remaining column(s).